#include <core/processor/processor_diff_drive.h>

Inheritance diagram for wolf::ProcessorDiffDrive:

Public Member Functions
	ProcessorDiffDrive (const YAML::Node &_params)

	~ProcessorDiffDrive () override

void	configure (SensorBasePtr _sensor) override

	WOLF_PROCESSOR_CREATE (ProcessorDiffDrive)

Public Member Functions inherited from wolf::ProcessorOdom2d
	ProcessorOdom2d (const YAML::Node &_params)

	~ProcessorOdom2d () override

void	configure (SensorBasePtr _sensor) override

	WOLF_PROCESSOR_CREATE (ProcessorOdom2d)

double	getMaxCovDet () const

bool	voteForKeyFrame () const override
	Vote for KeyFrame generation. More...

Public Member Functions inherited from wolf::ProcessorMotion
	ProcessorMotion (const std::string &_type, TypeComposite _state_types, SizeEigen _state_size, SizeEigen _delta_size, SizeEigen _delta_cov_size, SizeEigen _data_size, SizeEigen _calib_size, const YAML::Node &_params)

	~ProcessorMotion () override

virtual void	resetDerived ()

TimeStamp	getTimeStamp () const override
	Get the timestamp of the last motion. More...

VectorComposite	getState (StateKeys _keys="") const override
	Get the state at the last time. More...

VectorComposite	getState (const TimeStamp &_ts, StateKeys _keys="") const override
	Get the state at a specific time. More...

const Eigen::MatrixXd	getCurrentDeltaPreintCov () const
	Gets the delta preintegrated covariance from all integrations so far. More...

Motion	getMotion () const
	Provide the motion integrated so far. More...

Motion	getMotion (const TimeStamp &_ts) const
	Provide the motion integrated until a given timestamp. More...

CaptureMotionConstPtr	findCaptureContainingTimeStamp (const TimeStamp &_ts) const
	Finds the capture that contains the closest previous motion of _ts. More...

CaptureMotionPtr	findCaptureContainingTimeStamp (const TimeStamp &_ts)

void	setOrigin (FrameBasePtr _origin_frame)

FrameBasePtr	setOrigin (const VectorComposite &_x_origin, const TimeStamp &_ts_origin)

MotionBuffer &	getBuffer ()

const MotionBuffer &	getBuffer () const

virtual void	computeCurrentDelta (const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const double _dt, Eigen::VectorXd &_delta, Eigen::MatrixXd &_delta_cov, Eigen::MatrixXd &_jacobian_calib) const =0
	convert raw CaptureMotion data to the delta-state format More...

virtual void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _dt2, Eigen::VectorXd &_delta1_plus_delta2) const =0
	composes a delta-state on top of another delta-state More...

virtual void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _dt2, Eigen::VectorXd &_delta1_plus_delta2, Eigen::MatrixXd &_jacobian1, Eigen::MatrixXd &_jacobian2) const =0
	composes a delta-state on top of another delta-state, and computes the Jacobians More...

virtual void	statePlusDelta (const VectorComposite &_x, const Eigen::VectorXd &_delta, const double _dt, VectorComposite &_x_plus_delta) const =0
	composes a delta-state on top of a state More...

virtual Eigen::VectorXd	deltaZero () const =0
	Delta zero. More...

virtual Eigen::VectorXd	correctDelta (const Eigen::VectorXd &delta_preint, const Eigen::VectorXd &delta_step) const =0
	correct the preintegrated delta This produces a delta correction according to the appropriate manifold. More...

void	mergeCaptures (CaptureMotionPtr cap_prev, CaptureMotionPtr cap_target)
	merge two consecutive capture motions into the second one Merge two consecutive capture motions into the second one. The first capture motion is not removed. If the second capture has feature and factor emplaced, they are replaced by a new ones. More...

virtual Eigen::VectorXd	getCalibration (const CaptureBaseConstPtr _capture=nullptr) const =0

bool	hasCalibration () const

CaptureMotionConstPtr	getOrigin () const

CaptureMotionConstPtr	getLast () const

CaptureMotionConstPtr	getIncoming () const

CaptureMotionPtr	getOrigin ()

CaptureMotionPtr	getLast ()

CaptureMotionPtr	getIncoming ()

Eigen::MatrixXd	getUnmeasuredPerturbationCov () const

double	getMaxTimeSpan () const

double	getMaxBuffLength () const

double	getMaxDistTraveled () const

double	getMaxAngleTurned () const

void	setMaxTimeSpan (const double &_max_time_span)

void	setMaxBuffLength (const double &_max_buff_length)

void	setMaxDistTraveled (const double &_max_dist_traveled)

void	setMaxAngleTurned (const double &_max_angle_turned)

void	printHeader (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream, std::string _tabs="") const override

Public Member Functions inherited from wolf::ProcessorBase
void	startCaptureProfiling ()

void	stopCaptureProfiling ()

void	startKFProfiling ()

void	stopKFProfiling ()

virtual void	printProfiling (std::ostream &stream=std::cout) const

	ProcessorBase (const std::string &_type, const TypeComposite &_state_types, const YAML::Node &_params)
	constructor More...

	~ProcessorBase () override

virtual void	configure (SensorBasePtr _sensor)=0

virtual void	remove ()

bool	hasChildren () const override

const TypeComposite &	getStateTypes () const

StateKeys	getStateKeys () const

unsigned int	id () const

void	keyFrameCallback (FrameBasePtr _keyframe)
	notify a new keyframe made by another processor More...

void	captureCallback (CaptureBasePtr _capture)
	notify a new capture More...

SensorBaseConstPtr	getSensor () const

SensorBasePtr	getSensor ()

YAML::Node	getParams () const

bool	isMotionProvider () const

bool	applyLossFunction () const

bool	isVotingActive () const

double	getTimeTolerance () const

void	setApplyLossFunction (bool _apply_loss_function)

void	setVotingActive (bool _voting_active)

void	setTimeTolerance (double _time_tolerance)

void	setDebugVerboseLevel (const std::string &_debug_verbose_level)

std::string	getDebugVerboseLevel () const

bool	checkTimeTolerance (const TimeStamp &_ts1, const TimeStamp &_ts2) const

bool	checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp &_ts) const

bool	checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp &_ts) const

bool	checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap) const

virtual void	link (SensorBasePtr) final

virtual void	printHeader (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream, std::string _tabs="") const

void	print (int depth, bool factored_by, bool metric, bool state_blocks, std::ostream &stream=std::cout, std::string _tabs="") const

virtual CheckLog	localCheck (bool _verbose, std::ostream &_stream, std::string _tabs="") const

bool	check (CheckLog &_log, bool _verbose, std::ostream &_stream, std::string _tabs="") const

Public Member Functions inherited from wolf::NodeBase
	NodeBase (const std::string &_category, const std::string &_type="Undefined", const std::string &_name="")

virtual	~NodeBase ()=default

unsigned int	nodeId () const

std::string	getCategory () const

std::string	getType () const

std::string	getName () const

bool	isRemoving () const

virtual bool	hasChildren () const =0

void	setType (const std::string &_type)

void	setName (const std::string &_name)

ProblemConstPtr	getProblem () const

ProblemPtr	getProblem ()

Public Member Functions inherited from wolf::MotionProvider
	MotionProvider (const TypeComposite &_state_types, const YAML::Node &_params)
	Constructor for the MotionProvider class. More...

virtual	~MotionProvider ()
	Destructor for the MotionProvider class. More...

virtual TimeStamp	getTimeStamp () const =0
	Get the timestamp of the last motion. More...

virtual VectorComposite	getState (StateKeys _keys="") const =0
	Get the state at the last time. More...

virtual VectorComposite	getState (const TimeStamp &_ts, StateKeys _keys="") const =0
	Get the state at a specific time. More...

VectorComposite	getOdometry () const
	Get the odometry. More...

void	setOdometry (const VectorComposite &)
	Set the odometry. More...

bool	isStateProvider () const
	Check if the motion provider is configured as a state provider. More...

unsigned int	getOrder () const
	Get the order of the state provider. More...

void	setOrder (unsigned int)
	Set the order of the state provider. More...

Protected Member Functions
void	computeCurrentDelta (const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const double _dt, Eigen::VectorXd &_delta, Eigen::MatrixXd &_delta_cov, Eigen::MatrixXd &_jacobian_calib) const override
	convert raw CaptureMotion data to the delta-state format More...

CaptureMotionPtr	emplaceCapture (const FrameBasePtr &_frame_own, const SensorBasePtr &_sensor, const TimeStamp &_ts, const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib_preint, const CaptureBasePtr &_capture_origin) override
	emplace a CaptureMotion More...

void	emplaceFeaturesAndFactors (CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override
	emplace the features and factors corresponding to given capture and link them to the capture More...

Eigen::VectorXd	getCalibration (const CaptureBaseConstPtr _capture=nullptr) const override

void	setCalibration (const CaptureBasePtr _capture, const Eigen::VectorXd &_calibration) override

double	getRadPerTick () const

Protected Member Functions inherited from wolf::ProcessorOdom2d
void	computeCurrentDelta (const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const double _dt, Eigen::VectorXd &_delta, Eigen::MatrixXd &_delta_cov, Eigen::MatrixXd &_jacobian_calib) const override
	convert raw CaptureMotion data to the delta-state format More...

void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _Dt2, Eigen::VectorXd &_delta1_plus_delta2) const override
	composes a delta-state on top of another delta-state More...

void	deltaPlusDelta (const Eigen::VectorXd &_delta1, const Eigen::VectorXd &_delta2, const double _Dt2, Eigen::VectorXd &_delta1_plus_delta2, Eigen::MatrixXd &_jacobian1, Eigen::MatrixXd &_jacobian2) const override
	composes a delta-state on top of another delta-state, and computes the Jacobians More...

void	statePlusDelta (const VectorComposite &_x, const Eigen::VectorXd &_delta, const double _Dt, VectorComposite &_x_plus_delta) const override
	composes a delta-state on top of a state More...

Eigen::VectorXd	deltaZero () const override
	Delta zero. More...

Eigen::VectorXd	correctDelta (const Eigen::VectorXd &delta_preint, const Eigen::VectorXd &delta_step) const override
	correct the preintegrated delta This produces a delta correction according to the appropriate manifold. More...

CaptureMotionPtr	emplaceCapture (const FrameBasePtr &_frame_own, const SensorBasePtr &_sensor, const TimeStamp &_ts, const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib_preint, const CaptureBasePtr &_capture_origin_ptr) override
	emplace a CaptureMotion More...

void	emplaceFeaturesAndFactors (CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override
	emplace the features and factors corresponding to given capture and link them to the capture More...

Eigen::VectorXd	getCalibration (const CaptureBaseConstPtr _capture) const override

void	setCalibration (const CaptureBasePtr _capture, const Eigen::VectorXd &_calibration) override

Protected Member Functions inherited from wolf::ProcessorMotion
void	processCapture (CaptureBasePtr) override
	process an incoming capture More...

void	processKeyFrame (FrameBasePtr _keyframe_ptr) override
	process an incoming key-frame More...

bool	triggerInCapture (CaptureBasePtr) const override
	trigger in capture More...

bool	triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override
	trigger in key-frame More...

bool	storeKeyFrame (FrameBasePtr) override
	store key frame More...

bool	storeCapture (CaptureBasePtr) override
	store capture More...

bool	voteForKeyFrame () const override
	Vote for KeyFrame generation. More...

double	updateDt ()

void	integrateOneStep ()
	Make one step of motion integration. More...

void	reintegrateBuffer (CaptureMotionPtr _capture_ptr) const

void	splitBuffer (const wolf::CaptureMotionPtr &capture_source, const TimeStamp ts_split, const wolf::CaptureMotionPtr &capture_target) const

virtual void	preProcess ()

virtual void	bootstrap ()
	Bootstrap the processor with some initialization steps. More...

virtual void	postProcess ()

FrameBasePtr	computeProcessingStep ()

void	assertsCaptureMotion (CaptureMotionPtr _capture, std::string error_prefix) const

virtual CaptureMotionPtr	emplaceCapture (const FrameBasePtr &_frame_own, const SensorBasePtr &_sensor, const TimeStamp &_ts, const Eigen::VectorXd &_data, const Eigen::MatrixXd &_data_cov, const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib_preint, const CaptureBasePtr &_capture_origin_ptr)=0
	emplace a CaptureMotion More...

virtual void	emplaceFeaturesAndFactors (CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)=0
	emplace the features and factors corresponding to given capture and link them to the capture More...

virtual void	setCalibration (const CaptureBasePtr _capture, const Eigen::VectorXd &_calibration)=0

Motion	motionZero (const TimeStamp &_ts) const

Protected Member Functions inherited from wolf::ProcessorBase
virtual void	processCapture (CaptureBasePtr)=0
	process an incoming capture More...

virtual void	processKeyFrame (FrameBasePtr _keyframe_ptr)=0
	process an incoming key-frame More...

virtual bool	triggerInCapture (CaptureBasePtr) const =0
	trigger in capture More...

virtual bool	triggerInKeyFrame (FrameBasePtr) const =0
	trigger in key-frame More...

virtual bool	storeKeyFrame (FrameBasePtr)=0
	store key frame More...

virtual bool	storeCapture (CaptureBasePtr)=0
	store capture More...

virtual bool	voteForKeyFrame () const =0
	Vote for KeyFrame generation. More...

virtual bool	permittedKeyFrame () const final

void	setProblem (ProblemPtr) override

void	addTypes (const TypeComposite &)
	add state types More...

virtual void	setProblem (ProblemPtr _prob_ptr)

Protected Member Functions inherited from wolf::MotionProvider
void	addTypes (const TypeComposite &_state_types)
	Add types to the motion provider. More...

Protected Attributes
double	radians_per_tick_

Protected Attributes inherited from wolf::ProcessorMotion
ProcessingStep	processing_step_
	State machine controlling the processing step. More...

bool	bootstrapping_
	processor is bootstrapping More...

SizeEigen	x_size_
	The size of the state vector. More...

SizeEigen	data_size_
	the size of the incoming data More...

SizeEigen	delta_size_
	the size of the deltas More...

SizeEigen	delta_cov_size_
	the size of the delta covariances matrix More...

SizeEigen	calib_size_
	the size of the calibration parameters (TBD in derived classes) More...

CaptureMotionPtr	origin_ptr_

CaptureMotionPtr	last_ptr_

CaptureMotionPtr	incoming_ptr_

double	dt_
	Time step. More...

Eigen::VectorXd	x_
	current state More...

Eigen::VectorXd	delta_
	current delta More...

Eigen::MatrixXd	delta_cov_
	current delta covariance More...

Eigen::VectorXd	delta_integrated_
	integrated delta More...

Eigen::MatrixXd	delta_integrated_cov_
	integrated delta covariance More...

Eigen::VectorXd	calib_preint_
	calibration vector used during pre-integration More...

Eigen::MatrixXd	jacobian_delta_preint_
	jacobian of delta composition w.r.t previous delta integrated More...

Eigen::MatrixXd	jacobian_delta_
	jacobian of delta composition w.r.t current delta More...

Eigen::MatrixXd	jacobian_calib_
	jacobian of delta preintegration wrt calibration params More...

Eigen::MatrixXd	jacobian_delta_calib_
	jacobian of delta wrt calib params More...

Eigen::MatrixXd	unmeasured_perturbation_cov_
	Covariance of unmeasured DoF to avoid singularity. More...

Protected Attributes inherited from wolf::ProcessorBase
unsigned int	processor_id_

YAML::Node	params_

BufferFrame	buffer_frame_

BufferCapture	buffer_capture_

std::string	debug_verbose_level_

Protected Attributes inherited from wolf::NodeBase
ProblemWPtr	problem_ptr_

unsigned int	node_id_
	Node id. It is unique over the whole Wolf Tree. More...

std::string	node_category_
	Text label identifying the category of node ("SENSOR", "FEATURE", etc) More...

std::string	node_type_
	Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc) More...

std::string	node_name_

bool	is_removing_
	A flag for safely removing nodes from the Wolf tree. See remove(). More...

Protected Attributes inherited from wolf::MotionProvider
bool	is_state_provider_
	Whether this motion provider is configured as a state provider. More...

unsigned int	state_provider_order_
	If a state provider, the order to be used by the problem. More...

Additional Inherited Members
Public Types inherited from wolf::ProcessorMotion
enum	ProcessingStep { FIRST_TIME_WITHOUT_KF , FIRST_TIME_WITH_KF_BEFORE_INCOMING , FIRST_TIME_WITH_KF_ON_INCOMING , FIRST_TIME_WITH_KF_AFTER_INCOMING , RUNNING_WITHOUT_KF , RUNNING_WITH_KF_BEFORE_ORIGIN , RUNNING_WITH_KF_ON_ORIGIN , RUNNING_WITH_KF_AFTER_ORIGIN }

Constructor & Destructor Documentation

◆ ProcessorDiffDrive()

wolf::ProcessorDiffDrive::ProcessorDiffDrive ( const YAML::Node & _params )

◆ ~ProcessorDiffDrive()

wolf::ProcessorDiffDrive::~ProcessorDiffDrive ( )

override

Member Function Documentation

◆ computeCurrentDelta()

void wolf::ProcessorDiffDrive::computeCurrentDelta	(	const Eigen::VectorXd &	_data,
		const Eigen::MatrixXd &	_data_cov,
		const Eigen::VectorXd &	_calib,
		const double	_dt,
		Eigen::VectorXd &	_delta,
		Eigen::MatrixXd &	_delta_cov,
		Eigen::MatrixXd &	_jacobian_calib
	)		const

overrideprotectedvirtual

convert raw CaptureMotion data to the delta-state format

This function accepts raw data and time step dt, and computes the value of the delta-state and its covariance. Note that these values are held by the members delta_ and delta_cov_.

Parameters

_data	measured motion data
_data_cov	covariance
_dt	time step
_delta	computed delta
_delta_cov	covariance
_calib	current state of the calibrated parameters
_jacobian_calib	Jacobian of the delta wrt calib

Rationale:

The delta-state format must be compatible for integration using the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta(). See the class documentation for some Eigen::VectorXd suggestions.

The data format is generally not the same as the delta format, because it is the format of the raw data provided by the Capture, which is unaware of the needs of this processor.

Additionally, sometimes the data format is in the form of a velocity, or a higher derivative, while the delta is in the form of an increment. In such cases, converting from data to delta-state needs integrating the data over the period dt.

Two trivial implementations would establish:

If _data is an increment:

 delta_     = _data;
 delta_cov_ = _data_cov

If _data is a velocity:
```
 delta_     = _data * _dt
 delta_cov_ = _data_cov * _dt.
```
However, other more complicated relations are possible. In general, we'll have a nonlinear function relating delta_ to _data and _dt, as follows:

delta_ = f ( _data, _dt)

The delta covariance is obtained by classical uncertainty propagation of the data covariance, that is, through the Jacobians of f(),

delta_cov_ = F_data * _data_cov * F_data.transpose

where F_data = d_f/d_data is the Jacobian of f().

Tangent space: calibrate and go to se2 tangent --------------------------------------------—
Current delta: go to SE2 manifold --------------------------------------------—
delta covariance --------------------------------------------—
Jacobian of delta wrt calibration params --------------------------------------------—

Implements wolf::ProcessorMotion.

◆ configure()

void wolf::ProcessorDiffDrive::configure ( SensorBasePtr _sensor )

overridevirtual

Implements wolf::ProcessorBase.

◆ emplaceCapture()

CaptureMotionPtr wolf::ProcessorDiffDrive::emplaceCapture	(	const FrameBasePtr &	_frame_own,
		const SensorBasePtr &	_sensor,
		const TimeStamp &	_ts,
		const Eigen::VectorXd &	_data,
		const Eigen::MatrixXd &	_data_cov,
		const Eigen::VectorXd &	_calib,
		const Eigen::VectorXd &	_calib_preint,
		const CaptureBasePtr &	_capture_origin_ptr
	)

overrideprotectedvirtual

emplace a CaptureMotion

Parameters

_frame_own	frame owning the Capture to create
_sensor	Sensor that produced the Capture
_ts	time stamp
_data	a vector of motion data
_data_cov	covariances matrix of the motion data data
_calib	calibration vector
_calib_preint	calibration vector used during pre-integration
_capture_origin_ptr	capture acting as the origin of motions for the MorionBuffer of the created MotionCapture

Implements wolf::ProcessorMotion.

◆ emplaceFeaturesAndFactors()

void wolf::ProcessorDiffDrive::emplaceFeaturesAndFactors	(	CaptureBasePtr	_capture_origin,
		CaptureMotionPtr	_capture_own
	)

overrideprotectedvirtual

emplace the features and factors corresponding to given capture and link them to the capture

Parameters

_capture_own	the parent capture
_capture_origin	the capture constrained by this motion factor

Typical factors to add for a ProcessorMotionDerived can be:

A preintegrated motion factor – this is the main factor
A calibration drift factor – only for dynamic sensor calibration parameters
An instantaneous factor for observing non-typical states of the Frame – useful for complex dynamic robot models

Implements wolf::ProcessorMotion.

◆ getCalibration()

Eigen::VectorXd wolf::ProcessorDiffDrive::getCalibration ( const CaptureBaseConstPtr _capture = nullptr ) const

inlineoverrideprotectedvirtual

Implements wolf::ProcessorMotion.

◆ getRadPerTick()

double wolf::ProcessorDiffDrive::getRadPerTick ( ) const

inlineprotected

◆ setCalibration()

void wolf::ProcessorDiffDrive::setCalibration	(	const CaptureBasePtr	_capture,
		const Eigen::VectorXd &	_calibration
	)

inlineoverrideprotectedvirtual

Implements wolf::ProcessorMotion.

◆ WOLF_PROCESSOR_CREATE()

wolf::ProcessorDiffDrive::WOLF_PROCESSOR_CREATE ( ProcessorDiffDrive )

Member Data Documentation

◆ radians_per_tick_

double wolf::ProcessorDiffDrive::radians_per_tick_

protected

The documentation for this class was generated from the following files:

include/core/processor/processor_diff_drive.h
src/processor/processor_diff_drive.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Constructor & Destructor Documentation

◆ ProcessorDiffDrive()

◆ ~ProcessorDiffDrive()

Member Function Documentation

◆ computeCurrentDelta()

◆ configure()

◆ emplaceCapture()

◆ emplaceFeaturesAndFactors()

◆ getCalibration()

◆ getRadPerTick()

◆ setCalibration()

◆ WOLF_PROCESSOR_CREATE()

Member Data Documentation

◆ radians_per_tick_